/**
 * @file           : serial_data.cpp
 * @author         : fwy
 * @brief          :
 * @email          : 2643660853@qq.com
 * @date           : 23-1-18 上午1:06
 */

#include "serial_data.h"
using namespace std;

TransferData SerialData::read()
{
    // init
    if (!__is_init)
    {
        // 初始化接收数据
        TransferData transfer_data;
        transfer_data.rotation.yaw = 0;
        transfer_data.data_f =
            {
                // imu数据：p,q,g,a
                0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                // 左足数据:q,dq,tau
                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                // 右足数据:q,dq,tau
                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0

            };
        transfer_data.data_u8 =
            {
                0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U // 左右足mode
            };
        __last_data = transfer_data;
        __is_init = true;
    }
    // 接收数据
    vector<ReceiveData> receive_datas = __serial_port.readStruct<ReceiveData>(0x44, 0x55);
    // 判空
    if (!receive_datas.empty())
    {
        __receive_data = receive_datas.back();
        // 整合得到传递数据
        TransferData transfer_data;
        transfer_data.data_f = {
            __receive_data.position_x,
            __receive_data.position_y,
            __receive_data.position_z,
            __receive_data.quaternion_w,
            __receive_data.quaternion_x,
            __receive_data.quaternion_y,
            __receive_data.quaternion_z,
            __receive_data.gyroscope_x,
            __receive_data.gyroscope_y,
            __receive_data.gyroscope_z,
            __receive_data.accelerometer_x,
            __receive_data.accelerometer_y,
            __receive_data.accelerometer_z,

            __receive_data.q_L_hip,
            __receive_data.dq_L_hip,
            __receive_data.tauEst_L_hip,

            __receive_data.q_L_hip2,
            __receive_data.dq_L_hip2,
            __receive_data.tauEst_L_hip2,

            __receive_data.q_L_thign,
            __receive_data.dq_L_thign,
            __receive_data.tauEst_L_thign,

            __receive_data.q_L_toe,
            __receive_data.dq_L_toe,
            __receive_data.tauEst_L_toe,

            __receive_data.q_R_hip,
            __receive_data.dq_R_hip,
            __receive_data.tauEst_R_hip,

            __receive_data.q_R_hip2,
            __receive_data.dq_R_hip2,
            __receive_data.tauEst_R_hip2,

            __receive_data.q_R_thign,
            __receive_data.dq_R_thign,
            __receive_data.tauEst_R_thign,

            __receive_data.q_R_calf,
            __receive_data.dq_R_calf,
            __receive_data.tauEst_R_calf,

            __receive_data.q_R_toe,
            __receive_data.dq_R_toe,
            __receive_data.tauEst_R_toe

        };

        transfer_data.data_u8 = {
            __receive_data.mode_L_hip,
            __receive_data.mode_L_hip2,
            __receive_data.mode_L_thign,
            __receive_data.mode_L_calf,
            __receive_data.mode_L_toe,
            __receive_data.mode_R_hip,
            __receive_data.mode_R_hip2,
            __receive_data.mode_R_thign,
            __receive_data.mode_R_calf,
            __receive_data.mode_R_toe,
        };
        // 错误数据判断
        // if ((transfer_data.data_u8[0] != 0U && transfer_data.data_u8[0] != 1U) || (transfer_data.data_u8[1] != 0U && transfer_data.data_u8[1] != 1U))
        // {
        //     return __last_data;
        // }
        // 更新信息数据
        __last_data = transfer_data;
    }
    return __last_data;
}

void SerialData::write(const TransferData &transfer_data)
{
    static uint8_t end_flag = 0;
    // 数据准备
    uint8_t mode_L_hip = transfer_data.data_u8[0];
    uint8_t mode_L_hip2 = transfer_data.data_u8[1];
    uint8_t mode_L_thigh = transfer_data.data_u8[2];
    uint8_t mode_L_calf = transfer_data.data_u8[3];
    uint8_t mode_L_toe = transfer_data.data_u8[4];
    uint8_t mode_R_hip = transfer_data.data_u8[5];
    uint8_t mode_R_hip2 = transfer_data.data_u8[6];
    uint8_t mode_R_thigh = transfer_data.data_u8[7];
    uint8_t mode_R_calf = transfer_data.data_u8[8];
    uint8_t mode_R_toe = transfer_data.data_u8[9];

    float q_L_hip = transfer_data.data_f[0];   // L_hip_motor target position
    float dq_L_hip = transfer_data.data_f[1];  // L_hip_motor target velocity
    float tau_L_hip = transfer_data.data_f[2]; // L_hip_motor target torque
    float Kp_L_hip = transfer_data.data_f[3];  // L_hip_motor spring stiffness coefficient
    float Kd_L_hip = transfer_data.data_f[4];  // L_hip_motor damper coefficient
    // uint32[3] reserve_L_hip    // L_hip_motor target torque

    float q_L_hip2 = transfer_data.data_f[5];   // L_hip2_motor target position
    float dq_L_hip2 = transfer_data.data_f[6];  // L_hip2_motor target velocity
    float tau_L_hip2 = transfer_data.data_f[7]; // L_hip2_motor target torque
    float Kp_L_hip2 = transfer_data.data_f[8];  // L_hip2_motor spring stiffness coefficient
    float Kd_L_hip2 = transfer_data.data_f[9];  // L_hip2_motor damper coefficient
    // uint32[3] reserve_L_hip2    // L_hip2_motor target torque

    float q_L_thigh = transfer_data.data_f[10];   // L_thigh_motor target position
    float dq_L_thigh = transfer_data.data_f[11];  // L_thigh_motor target velocity
    float tau_L_thigh = transfer_data.data_f[12]; // L_thigh_motor target torque
    float Kp_L_thigh = transfer_data.data_f[13];  // L_thigh_motor spring stiffness coefficient
    float Kd_L_thigh = transfer_data.data_f[14];  // L_thigh_motor damper coefficient
    // uint32[3] reserve_L_thigh    // L_thigh motor target torque

    float q_L_calf = transfer_data.data_f[15];   // L_calf_motor target position
    float dq_L_calf = transfer_data.data_f[16];  // L_calf_motor target velocity
    float tau_L_calf = transfer_data.data_f[17]; // L_calf_motor target torque
    float Kp_L_calf = transfer_data.data_f[18];  // L_calf_motor spring stiffness coefficient
    float Kd_L_calf = transfer_data.data_f[19];  // L_calf_motor damper coefficient
    // uint32[3] reserve_L_calf    // L_calf motor target torque

    float q_L_toe = transfer_data.data_f[20];   // L_toe_motor target position
    float dq_L_toe = transfer_data.data_f[21];  // L_toe_motor target velocity
    float tau_L_toe = transfer_data.data_f[22]; // L_toe_motor target torque
    float Kp_L_toe = transfer_data.data_f[23];  // L_toe_motor spring stiffness coefficient
    float Kd_L_toe = transfer_data.data_f[24];  // L_toe_motor damper coefficient
    /// uint32[3] reserve_L_toe    // L_toe motor target torque

    // ##############################LR分界线#####################################################

    float q_R_hip = transfer_data.data_f[25];   // R_hip_motor target position
    float dq_R_hip = transfer_data.data_f[26];  // R_hip_motor target velocity
    float tau_R_hip = transfer_data.data_f[27]; // R_hip_motor target torque
    float Kp_R_hip = transfer_data.data_f[28];  // R_hip_motor spring stiffness coefficient
    float Kd_R_hip = transfer_data.data_f[29];  // R_hip_motor damper coefficient
    // uint32[3] reserve_R_hip    // R_hip_motor target torque

    float q_R_hip2 = transfer_data.data_f[30];   // R_hip2_motor target position
    float dq_R_hip2 = transfer_data.data_f[31];  // R_hip2_motor target velocity
    float tau_R_hip2 = transfer_data.data_f[32]; // R_hip2_motor target torque
    float Kp_R_hip2 = transfer_data.data_f[33];  // R_hip2_motor spring stiffness coefficient
    float Kd_R_hip2 = transfer_data.data_f[34];  // R_hip2_motor damper coefficient
    // uint32[3] reserve_R_hip2    // R_hip2_motor target torque

    float q_R_thigh = transfer_data.data_f[35];   // R_thigh_motor target position
    float dq_R_thigh = transfer_data.data_f[36];  // R_thigh_motor target velocity
    float tau_R_thigh = transfer_data.data_f[37]; // R_thigh_motor target torque
    float Kp_R_thigh = transfer_data.data_f[38];  // R_thigh_motor spring stiffness coefficient
    float Kd_R_thigh = transfer_data.data_f[39];  // R_thigh_motor damper coefficient
    // uint32[3] reserve_R_thigh    // R_thigh motor target torque

    float q_R_calf = transfer_data.data_f[40];   // R_calf_motor target position
    float dq_R_calf = transfer_data.data_f[41];  // R_calf_motor target velocity
    float tau_R_calf = transfer_data.data_f[42]; // R_calf_motor target torque
    float Kp_R_calf = transfer_data.data_f[43];  // R_calf_motor spring stiffness coefficient
    float Kd_R_calf = transfer_data.data_f[44];  // R_calf_motor damper coefficient
    // uint32[3] reserve_R_calf    # R_calf motor target torque

    float q_R_toe = transfer_data.data_f[45];   // R_toe_motor target position
    float dq_R_toe = transfer_data.data_f[46];  // R_toe_motor target velocity
    float tau_R_toe = transfer_data.data_f[47]; // R_toe_motor target torque
    float Kp_R_toe = transfer_data.data_f[48];  // R_toe_motor spring stiffness coefficient
    float Kd_R_toe = transfer_data.data_f[49];  // R_toe_motor damper coefficient
    // uint32[3] reserve_R_toe    # R_toe motor target torque
    end_flag++;

    // 整合得到发送数据
    __send_data = {mode_L_hip, mode_L_hip2, mode_L_thigh, mode_L_calf, mode_L_toe, mode_R_hip, mode_R_hip2,
                   mode_R_thigh, mode_R_calf, mode_R_toe,
                   q_L_hip, dq_L_hip, tau_L_hip, Kp_L_hip, Kd_L_hip,
                   q_L_hip2, dq_L_hip2, tau_L_hip2, Kp_L_hip2, Kd_L_hip2,
                   q_L_thigh, dq_L_thigh, tau_L_thigh, Kp_L_thigh, Kd_L_thigh,
                   q_L_calf, dq_L_calf, tau_L_calf, Kp_L_calf, Kd_L_calf,
                   q_L_toe, dq_L_toe, tau_L_toe, Kp_L_toe, Kd_L_toe,
                   q_R_hip, dq_R_hip, tau_R_hip, Kp_R_hip, Kd_R_hip,
                   q_R_hip2, dq_R_hip2, tau_R_hip2, Kp_R_hip2, Kd_R_hip2,
                   q_R_thigh, dq_R_thigh, tau_R_thigh, Kp_R_thigh, Kd_R_thigh,
                   q_R_calf, dq_R_calf, tau_R_calf, Kp_R_calf, Kd_R_calf,
                   q_R_toe, dq_R_toe, tau_R_toe, Kp_R_toe, Kd_R_toe, end_flag};
    // 写入串口
    __serial_port.writeStruct<SendData>(__send_data);
}

// TODO
bool SerialData::isOpened()
{
    if (__serial_port.isOpened())
        return true;
    else
        return false;
}
